%%
%% 准备

clc
clear
close all
%% 系统参数设定
% 系统模型
% 信源$$\alpha-\beta-\gamma$$参数

var_abg = struct('alpha',   [],...
    'beta',    [],...
    'gamma',   [],...
    'A',       [],...
    'mu',      [],...
    'epsilon', []);
var_abg.A = sqrt(0.2);
var_abg.mu = 0;
var_abg.epsilon = var_abg.A^2 / 3;
var_abg.alpha = log(var_abg.A) - 1;
var_abg.beta  = 0;
var_abg.gamma = 0;

% 信道参数
%%
% * 朗博系数$m$
% * PD面积$A_R$
% * 电光效率$\eta_c$
% * 光电效率$\eta_\ell$
% * 辅助变量$$\mu$$
% * 噪声标准差$\sigma_c$


var_channel = struct('m', [],...
    'A_R', [],...
    'eta_c', [],...
    'eta_l', [],...
    'mu',[],...
    'sigma_c', [],...
    'v', [],...
    'N_BS', []);

var_channel.m = 1;
var_channel.A_R = 0.01;
var_channel.eta_l = 1;
var_channel.eta_c = 1;
var_channel.mu = (var_channel.m + 1) * var_channel.A_R * var_channel.eta_c * var_channel.eta_l / 2 / pi;
var_channel.sigma_c = 5e-8;
% LED灯位置
%%
% * 位置$$\left(x, y, z\right)$$数组
% * 灯的数量

var_channel.v  = {[1 1 3]',[3 1 3]',[5 1 3]',[1 3 3]', [3 3 3]', [5 3 3]', [1 5 3]', [3 5 3]', [5 5 3]'};
% var_channel.v  = {[1 1 3]', [2 1 3]', [3 1 3]', [4 1 3]', [5 1 3]',...
%                   [1 2 3]', [2 2 3]', [3 2 3]', [4 1 3]', [5 2 3]',...
%                   [1 3 3]', [2 3 3]', [3 3 3]', [4 3 3]', [5 3 3]',...
%                   [1 4 3]', [2 4 3]', [3 4 3]', [4 4 3]', [5 4 3]',...
%                   [1 5 3]', [2 5 3]', [3 5 3]', [4 5 3]', [5 5 3]'};
var_channel.N_BS = length(var_channel.v);

% 带宽
W = 20e6;
% 子帧帧长

T_b = 0.05;
% 定位噪声标准差

sigma_p = var_channel.sigma_c;
% 用户参数
% 真实位置

u = [2.6;2.6;1.7];
% 朝向方向向量

l = [0; 0; 1];
% 视场角FoV

angle_FoV = 2 * pi/3;

% 接收端可以收到的LED
[v_recv, index_v_recv] = Find_Recv_LED(u, l, angle_FoV, var_channel.v, var_channel.N_BS);

% v_recv = var_channel.v;
% index_v_recv = 1:var_channel.N_BS;

% 功率约束
%%
% * 总功率约束
% * 平均电功率约束
% * 最大光功率约束
% * 直流偏置约束

var_power = struct('P_total', [],...
    'P_e_ave',   [],...
    'P_o_max',   [],...
    'I_DC',      [],...
    'P_p_upper', [],...
    'P_c_upper', []);

var_power.I_DC = 2 * var_abg.A;
var_power.P_e_ave = 5 * var_power.I_DC^2;
var_power.P_o_max = 5 * var_power.I_DC;

% var_power.P_p_upper = min([var_power.I_DC^2 / var_abg.A^2, (var_power.P_e_ave - var_power.I_DC^2) / var_abg.epsilon, (var_power.P_o_max - var_power.I_DC)^2 / var_abg.A^2]);
% var_power.P_c_upper = min([var_power.I_DC^2 / var_abg.A^2, (var_power.P_e_ave - var_power.I_DC^2) / var_abg.epsilon, (var_power.P_o_max - var_power.I_DC)^2 / var_abg.A^2]);
% var_power.P_total = (var_channel.N_BS * var_power.P_p_upper + var_power.P_c_upper) * 0.1;


var_power.P_p_upper = 1e100;
var_power.P_c_upper = 1e100;
var_power.P_total = 1000;

% 匿名函数定义

f_v_opt                      = @(u) Find_Opt_LED(u, v_recv);
f_crlb_inv_fim               = @(u, P_p) Calc_CRLB_inv_FIM_mex(u, P_p, var_channel, v_recv, index_v_recv, var_abg, var_power, T_b, sigma_p, W);
f_rate                       = @(u, v_opt, P_c) Calc_Rate(u, v_opt, P_c, var_channel, var_abg, W);
f_power_allocation_p_matlab = @(P_p_max, u) Power_Allocation_P_matlab(P_p_max, u, var_channel, v_recv, index_v_recv, var_abg, var_power, T_b, sigma_p, W);
f_power_allocation_p_cvx = @(P_p_max, u) Power_Allocation_P_cvx(P_p_max, u, var_channel, v_recv, index_v_recv, var_abg, var_power, T_b, sigma_p, W);
f_power_allocation_c = @(R_bar, P_c_max, u, v_opt) Power_Allocation_C_mex(R_bar, P_c_max, u, v_opt, var_channel, var_abg, W);

% 确定最优LED
v_opt = f_v_opt(u);

P_p_max_list = 0:0.1:var_power.P_total;
P_c_max_list = var_power.P_total - P_p_max_list;
N = length(P_p_max_list);
N_MONT = 1000000;

crlb = nan(1, N);
inv_FIM_list = cell(1, N);
Prob_out = nan(1, N);
average_rate = nan(1, N);

R_bar = 10e6;

tic
parfor i = 1:N
    P_p = f_power_allocation_p_matlab(P_p_max_list(i), u);% 功率分配
    [crlb(i), inv_FIM_list{i}] = f_crlb_inv_fim(u, P_p); % 计算定位误差
end
toc
for i = 1:N
    fprintf('%04d / %d', i, N)
    inv_FIM = inv_FIM_list{i};
    P_c_max =  P_c_max_list(i);
    outage_counter = 0;
    sum_rate = 0;
    parfor j = 1:N_MONT
        e_p = [chol(inv_FIM) * randn(2, 1); zeros(1, 1)]; %产生e_p样本
        u_hat = u + e_p;
        P_c_Allocated = f_power_allocation_c(R_bar, P_c_max, u_hat, v_opt);
        rate = f_rate(u, v_opt, P_c_Allocated); % 验证可达速率
        if rate < R_bar || isnan(rate)
            outage_counter = outage_counter + 1;
        end
        if ~isnan(rate)
            sum_rate = sum_rate + rate;
        end
    end
    Prob_out(i) = outage_counter / N_MONT;
    average_rate(i) = sum_rate / N_MONT;
    fprintf(' | root-CRLB = %f | Prob_out = %f | Ave. Rate = %f\n',sqrt(crlb(i)), Prob_out(i), average_rate(i))
end

if P_p_max_list(1) == 0
    crlb(1) = inf;
    Prob_out(1) = 1;
    average_rate(1) = 0;
end

yyaxis left
plot(sqrt(crlb), Prob_out)
ylabel('Outage Probability')

yyaxis right
plot(sqrt(crlb), average_rate)
ylabel('Average Rate')

xlabel('$\sqrt{\mathrm{CRLB}}$', 'Interpreter', 'latex')

% save('tradeoff_nonrobust_8.mat', 'crlb', 'Prob_out', 'average_rate', 'R_bar', 'var_power')
%% 功能函数
% 与位置有关的函数
% 可以接收到的灯

function [v_recv, index_LED_recv_list, flag_LED_recv_list] = Find_Recv_LED(u, l, FoV, v_list, N)
flag_LED_recv_list = false(N, 1);
for i = 1:N
    if acos(dot((v_list{i} - u), l) / norm((v_list{i} - u)) / norm(l)) <= FoV / 2
        flag_LED_recv_list(i) = true;
    end
end
v_recv = v_list(flag_LED_recv_list);
index_LED_recv_list = 1:N;
index_LED_recv_list = index_LED_recv_list(flag_LED_recv_list);
end

% 寻找最近距离的LED
function v_opt = Find_Opt_LED(u, v_list)
[~, index_v_opt] = min(sum((cell2mat(v_list) - u).^2));
v_opt = v_list{index_v_opt};
end

% 可达速率

function rate = Calc_Rate(u, v_opt, P_c, var_channel, var_abg, W)
if sum(isnan(u)) >= 1 || sum(isnan(P_c)) >= 1
    rate = nan;
    return
end
g = var_channel.mu * (v_opt(3) - u(3))^(var_channel.m + 1) / norm(v_opt - u)^(var_channel.m + 3); % 信道增益

rate = W * log2(1 + (g^2 * P_c * exp(1 + 2 * (var_abg.alpha + var_abg.gamma * var_abg.epsilon)))/ (2 * pi * W * var_channel.sigma_c^2));
end